includes Header;

module ProcessM
{
	provides
	{
    interface StdControl;
	}
	uses
	{
		interface StdControl as CommControl;

    interface StdControl as LogControl;

		interface Leds;

		interface SendMsg as TopoSendMsg;
		interface ReceiveMsg as TopoReceiveMsg;
    
    interface SendMsg as CmdSendMsg;
    interface ReceiveMsg as CmdReceiveMsg;

    interface SendMsg as DataSendMsg;
    interface ReceiveMsg as DataReceiveMsg;

    interface SendMsg as LogSendMsg;
    interface ReceiveMsg as LogReceiveMsg;

    interface Timer as BootTimer;

    interface Timer as ClockTimer;
    interface Timer as TopoTimer;
    interface Timer as TransferTimer;

    interface Random;

		interface LoggerRead;
		interface LoggerWrite;
	}
}
implementation
{
  Type type;
  uint8_t sync_flag;        //Flag of time sync
  uclock_t local_clock;     //Local clock of the node

  keys_t mask;              //Keys flag depends on TOS_LOCAL_ADDRESS

  neighbour_t neighbours;   //Neighours. Each 2 bits presents one neighbour
  keys_t keys;              //Key fregments. Each bit present one fregment

  TOS_Msg data;             //Messafe buffer for sending control cmd
  TOS_Msg frags_data;       //Message buffer for sending fragments

  uint8_t k_order[MAX_NODES + 1];   //Random order for forwardding fragments

  MsgNode msg_buf[MAX_NODES + 1];   //FIFO queue for fragment messages
  uint8_t head;                     //The index of queue head, pointing to next message to be sent
  uint8_t tail;                     //The index of queue head, pointing to empty buffer for the new message

  uint32_t send_cnt;                //Count of messages to be sent
  uint32_t send_suc;                //Count of messages sent successfully

  LogMsg log_buf[MAX_LOG_BUFFER];
  uint8_t log_head;
  uint8_t log_tail;

  LogMsg temp_log;

  uint8_t event_uni_flag;
  uint8_t event_fwd_flag;

  uint32_t encounter[MAX_NODES + 1];
  
  /* Local adress of the node */
  //TOS_LOCAL_ADDRESS

  /* Shuffle the fragments' order */
  void shuffle(uint8_t *array, uint8_t from,uint8_t to)
  {
    uint8_t i,temp,r_index;
    uint16_t rand_num;
    for(i = to - 1;i >= from;i--)
    {
      rand_num = call Random.rand();
      r_index = rand_num%(to -from) + from;
      temp = array[i];
      array[i] = array[r_index];
      array[r_index] = temp;
    }
  }

  /* Convert a keys_t to a binary string */
  void itob(keys_t num,char *r)
  {
    uint32_t imask;
    uint8_t i;
    uint32_t iflag = 1;

    imask = iflag << 31;
    *r = '\0';

    for(i = 0;i < 32;i++)
    {
      //dbg(DBG_USR1,"%d",imask & num);
      if((imask & num) == 0)
      {
        strcat(r,"0");
      }
      else
      {
        strcat(r,"1");
      }

      imask = imask >> 1;

      if(i%2 == 1)
      {
        strcat(r," ");
      }
    }
  }

  /* Convert an integer to a string */
  void itos(uint16_t a,char *r)
  {
    if(a/10!=0)
    {
      *r = a/10 + 48;
      *(r + 1) = a%10 + 48;
      *(r + 2) = '\0';
    }
    else
    {
      *r = a%10 + 48;
      *(r + 1) = '\0';
    }
  }

  /* Print a message with the format of DEBUG information */
  void printMsg(KUMsg *m)
  {
    /*
      MsgType type;
      addr16_t src;
      addr16_t dst;
      keys_t keys;
    */
    char s[100];
    switch(m->type)
    {
      case SYNC:  s[0] = 'S';break;
      case TOPO:  s[0] = 'T';break;
      case KEYS:  s[0] = 'K';break;
      case LOG_REQUEST:  s[0] = 'L';break;
    }
    s[1] = '\0';

    itos(m->src,s + strlen(s));

    strcat(s,"->");

    itos(m->dst,s + strlen(s));

    strcat(s,":");

    itob(m->keys,s + strlen(s));

    //dbg(DBG_USR1,"MSG:%s\n",s);
  }

  /* Print the address with the format of DEBUG information */
  void printAddr(addr16_t a)
  {
    char r[3];
    if(a/10!=0)
    {
      *r = a/10 + 48;
      *(r + 1) = a%10 + 48;
      *(r + 2) = '\0';
    }
    else
    {
      *r = a%10 + 48;
      *(r + 1) = '\0';
    }

    //dbg(DBG_USR1,"Addr:%u-%s\n",a,r);
  }

  /* Print the key fragment set with the format of DEBUG information */
  void printKeys(keys_t a)
  {
    uint32_t imask;
    uint8_t i;
    char r[50];
    uint32_t iflag = 1;

    imask = iflag << 31;
    *r = '\0';

    for(i = 0;i < 32;i++)
    {
      if((imask & a) == 0)
      {
        strcat(r,"0");
      }
      else
      {
        strcat(r,"1");
      }

      imask = imask >> 1;

      if(i%2 == 1)
      {
        strcat(r," ");
      }
    }

    //dbg(DBG_USR1,"Keys:%u-%s\n",a,r);
  }

  /* Print the neighbour set with the format of DEBUG information */
  void printNeighbours(neighbour_t a)
  {
    uint32_t imask;
    uint8_t i;
    char r[50];
    uint32_t iflag = 2;

    imask = iflag << 30;
    *r = '\0';

    for(i = 0;i < 16;i++)
    {
      //dbg(DBG_USR1,"%d",imask & num);
      if((imask & a) == 0)
      {
        strcat(r,"00");
      }
      else
      {
        strcat(r,"10");
      }

      imask = imask >> 2;
      
      strcat(r," ");
    }

    //dbg(DBG_USR1,"Neighbours:%u-%s at %d\n ",a,r,local_clock);
  }

  /* Print log information */
  void printLog(LogMsg *l)
  {
    /*
    typedef struct
    {
      LogType lType;
      uclock_t lTime;
      addr16_t lSrc;
      addr16_t lDst;
      uint32_t data;
    }LogMsg;
    */
    char s[100];
    if(l->lType == ENCOUNTER) s[0] = 'E';
    if(l->lType == EVENT_FWD) s[0] = 'F';
    if(l->lType == EVENT_UNI) s[0] = 'U';

    sprintf(s + 1,"%u:%u->%u:%u\0",l->lTime,l->lSrc,l->lDst,l->data);

    dbg(DBG_USR1,"LOG:%s\n",s);
  }

  /* Initial for the node */
  void init()
  {
    uint8_t i;
    sync_flag = 0;
    local_clock = 0;
    neighbours = 0;
    type = INFECTED;
    mask = 3<<((TOS_LOCAL_ADDRESS - 1) * 2);
    //srand((unsigned)time(NULL));
    if(TOS_LOCAL_ADDRESS == SINK_ADDR)
    {
      keys = ~0;
    }
    else
    {
      keys = 0;
    }

    for(i = 1;i <= 16;i++)
    {
      if(TOS_LOCAL_ADDRESS == SINK_ADDR)
      {
        k_order[i] = i ;
      }
      else
      {
        k_order[i] = 0;
      }
    }

    k_order[0] = MAX_NODES;

    //dbg(DBG_USR1,"Before shuffle ...\n");
    shuffle(k_order,1,MAX_NODES + 1);

    //dbg(DBG_USR1,"Keys:%x\n",keys);

    head = 0;
    tail = 0;

    log_head = 0;
    log_tail = 0;

    send_cnt = 0;
    send_suc = 0;

    for(i = 0;i <= MAX_NODES;i++)
    {
      encounter[i] = 0;
    }

    event_fwd_flag = 1;
    event_uni_flag = 1;
  }

  /* Update neigours by <I>addr</I>. <I>addr</I> rangs from 1 to 16.*/
  void updateNeighbours(addr16_t addr)
  {
    uint32_t addr_bit;
    uint32_t addr_mask = 2;
    if(addr != SINK_ADDR)
    {
      addr_bit = addr_mask << ((addr - 1) * 2);

      neighbours = neighbours|addr_bit;
    }
  }

  /* SINK or FORWARD node forwards key fragments to <I>dst</I> */
  keys_t forwardKeys(addr16_t dst)
  {
    keys_t result;
    uint8_t temp;
    uint8_t i;

    shuffle(k_order,1,k_order[0]);
    for(i = 1;i < k_order[0];i++)
    {
      if(k_order[i] == dst * 2 || k_order[i] == dst * 2 - 1)
      {
        k_order[i] = k_order[k_order[MAX_NODES] - 1];
        k_order[MAX_NODES]--;
      }
    }

    result = 3 >> ((dst - 1) * 2);

    if(k_order[1] != 0)
    {
      temp = k_order[1];
      k_order[1] = k_order[k_order[0] - 1];
      k_order[k_order[0] - 1] = temp;

      result = result | (1 << (temp - 1));
    }

    if(k_order[1] != 0)
    {
      temp = k_order[1];
      k_order[1] = k_order[k_order[MAX_NODES] - 1];
      k_order[k_order[0] - 1] = temp;
      result = result | (1 << (temp - 1));
    }

    return result;
  }

  /* Task about time sync */
  task void doSync()
  {
    if(sync_flag == 0)
    {
      sync_flag = 1;

      call ClockTimer.start(TIMER_REPEAT,ONE_SENCOND);
      //dbg(DBG_USR1,"Clock start\n");

      call TopoTimer.start(TIMER_REPEAT,TOPO_TIMER_INTERVAL);
      //dbg(DBG_USR1,"Topo detect start\n");
    }
  }

  /*SINK Broadcast the request for time sync*/
  task void bcastSync()
  {
    result_t retval;
    KUMsg* sync_msg = (KUMsg*)(data.data);

    sync_msg->type = SYNC;
    sync_msg->src = TOS_LOCAL_ADDRESS;

    retval = call CmdSendMsg.send(TOS_BCAST_ADDR,sizeof(KUMsg),&data);
  }

  /*SINK Broadcast the request for log*/
  task void bcastLoggerRequest()
  {
    KUMsg* log_request_msg = (KUMsg*)(data.data);

    log_request_msg->type = LOG_REQUEST;

    call CmdSendMsg.send(TOS_BCAST_ADDR,sizeof(KUMsg),&data);
  }

  /* Broadcast the TOPO request */
  task void bcastTopo()
  {
    //TOS_Msg topo_data;
    KUMsg* topo_msg = (KUMsg*)(data.data);

    topo_msg->type = TOPO;

    topo_msg->src = TOS_LOCAL_ADDRESS;

    call TopoSendMsg.send(TOS_BCAST_ADDR,sizeof(KUMsg),&data);
    //dbg(DBG_USR1,"Topo detect Start\n");
  }

  /* Task about sending fragment set */
  task void sendData()
  {
    send_cnt++;
    if(head != tail)
    {
      //frags_data.data = 
      //memcpy(new_message.data,&this_data,sizeof(uint16_t));
      memcpy(frags_data.data,&(msg_buf[head].node),sizeof(KUMsg));
      call DataSendMsg.send(msg_buf[head].node.dst,sizeof(KUMsg),&frags_data);
      head++;
      //dbg(DBG_USR1,"K%x-->%d\n",head->node.keys,head->node.dst);
      //printMsg(&(head->node));
    }
  }

  void logEncounter()
  {
    /*
      typedef struct
      {
        LogType lType;
        uclock_t lTime;
        addr16_t lSrc;
        addr16_t lDst;
        uint32_t data;
      }LogMsg;
    */
    uint16_t i;
    for(i = 0;i < MAX_NODES + 1;i++)
    {
      if(i != TOS_LOCAL_ADDRESS)
      {
        atomic
        {
          log_buf[log_tail].lType = ENCOUNTER;
          log_buf[log_tail].lTime = local_clock;
          log_buf[log_tail].lSrc  = TOS_LOCAL_ADDRESS;
          log_buf[log_tail].lDst  = i;
          log_buf[log_tail].data  = encounter[i];

          log_tail++;
        }
      }
    }
  }

  /* Task about writing log */
  task void writeLog()
  {
    if(log_head != log_tail)
    {
      call LoggerWrite.append((uint8_t *)(log_buf + log_head));
      //printLog(log_buf + log_head);
      log_head++;
    }
  }

  task void readLog()
  {
    /*
    if(head != tail)
    {
      memcpy(frags_data.data,&(msg_buf[head].node),sizeof(LogMsg));
      call DataSendMsg.send(msg_buf[head].node.dst,sizeof(LogMsg),&frags_data);
      head++;
    }
    */
    result_t retval;
    retval = call LoggerRead.readNext((uint8_t *)(log_buf + log_tail));
    log_tail++;
  }

	command result_t StdControl.init()
  {
    result_t retval;

    retval = call CommControl.init();

    retval = call Leds.init();

    retval = call Random.init();

    retval = call LogControl.init();

    init();

    return SUCCESS;
  }

  command result_t StdControl.start()
  {
    result_t retval;

    retval = call CommControl.start();
    //dbg(DBG_USR1,"Comm Start %d\n",retval);

    retval = call BootTimer.start(TIMER_ONE_SHOT,3000);
    //dbg(DBG_USR1,"BootTimer start %d\n",retval);

    retval = call LogControl.start();

    retval = call LoggerWrite.resetPointer();

    if(TOS_LOCAL_ADDRESS == CONTROLMOTE)
    {
      post bcastLoggerRequest();
    }

    return SUCCESS;
  }

  command result_t StdControl.stop()
  {
    call CommControl.stop();
    call LogControl.stop();
    return SUCCESS;
  }

  event result_t ClockTimer.fired()
  {
    atomic
    {
      local_clock = local_clock + 1;
    }
    call Leds.redToggle();

    return SUCCESS;
  }

  event result_t BootTimer.fired()
  {
    //dbg(DBG_USR1,"BootTimer fired...\n");
    if(TOS_LOCAL_ADDRESS == SINK_ADDR)
    {
      //dbg(DBG_USR1,"Broadcast Sync ...\n");
      post bcastSync();
    }

    return SUCCESS;
  }

  event result_t TopoTimer.fired()
  {
    //Clear the neighbours
    neighbours = 0;

    call TransferTimer.start(TIMER_ONE_SHOT,TRANSFER_INTERVAL);
    
    //Start topo detect
    post bcastTopo();

    return SUCCESS;
  }

  event result_t TransferTimer.fired()
  {
    neighbour_t temp_neighbours;
    addr16_t i;
    keys_t frags;

    atomic
    {
      temp_neighbours = neighbours;
    }

    //dbg(DBG_USR1,"Neibours:%8x-%d\n",temp_neighbours,local_clock);
    printNeighbours(temp_neighbours);

    /* Clear message buffer*/
    head = 0;
    tail = 0;

    for(i = 1;i <= 16;i++)
    {
      if(temp_neighbours & 2)
      {
        if(TOS_LOCAL_ADDRESS == SINK_ADDR)
        {
          frags = forwardKeys(i);
          frags = frags | (3<<((i - 1) * 2));
          printKeys(frags);
          atomic
          {
            msg_buf[tail].node.type = KEYS;
            msg_buf[tail].node.src = TOS_LOCAL_ADDRESS;
            msg_buf[tail].node.dst = i;
            msg_buf[tail].node.keys = frags;
            msg_buf[tail].next = msg_buf + tail + 1;
            tail++;
          }
        }
        else
        {
          if(type == FORWARD)
          {
            frags = keys & (3<<((i - 1) * 2));
            if(frags)
            {
              //call DataSendMsg.send(i,sizeof(KUMsg),&frags_data);
              atomic
              {
                msg_buf[tail].node.type = KEYS;
                msg_buf[tail].node.src = TOS_LOCAL_ADDRESS;
                msg_buf[tail].node.dst = i;
                msg_buf[tail].node.keys = frags;
                msg_buf[tail].next = msg_buf + tail + 1;
                tail++;
              }
            }
          }
        }
      }
      temp_neighbours = temp_neighbours >> 2;
    }

    post sendData();
    return SUCCESS;
  }

  void generateKeysMsg(keys_t k)
  {
    KUMsg* f = (KUMsg*)(frags_data.data);
    f->type = KEYS;
    f->src = TOS_LOCAL_ADDRESS;
    f->keys = k;
  }

  event result_t TopoSendMsg.sendDone(TOS_MsgPtr msg, result_t status)
  {
    //dbg(DBG_USR1,"Topo detect Senddone %d\n");
    return status;
  }

  event result_t CmdSendMsg.sendDone(TOS_MsgPtr msg, result_t status)
  {

    //dbg(DBG_USR1,"Sync Senddone %d\n");

    /* Sink Clock start */
    post doSync();

    return status;
  }

  event result_t DataSendMsg.sendDone(TOS_MsgPtr msg, result_t status)
  {
    KUMsg *m = (KUMsg*)(msg->data);
    //dbg(DBG_USR1,"sendDone data frags to %d:%x\n",msg->addr,m->keys);
    if(TOS_LOCAL_ADDRESS == SINK_ADDR)
    {
      //dbg(DBG_USR1,">>>>>>>>");
      printMsg(m);

    }

    send_suc++;

    if(TOS_LOCAL_ADDRESS == SINK_ADDR)
    {
      //dbg(DBG_USR1,"SEND RATE:%u %u\n",send_suc,send_cnt);
    }
    
    post sendData();

    return status;
  }

  event result_t LogSendMsg.sendDone(TOS_MsgPtr msg,result_t status)
  {
    post readLog();

    return SUCCESS;
  }

  //event TOS_MsgPtr receive(TOS_MsgPtr m);
  event TOS_MsgPtr TopoReceiveMsg.receive(TOS_MsgPtr m)
  {
    KUMsg* topo_msg = (KUMsg*)(m->data);

    printAddr(topo_msg->src);
    updateNeighbours(topo_msg->src);

    encounter[topo_msg->src]++;
    return m;
  }

  event TOS_MsgPtr CmdReceiveMsg.receive(TOS_MsgPtr m)
  {
    KUMsg* cmd_msg = (KUMsg*)m->data;

    //Sync Request
    if(cmd_msg->type == SYNC)
    {
      //Mobile node start the local clock
      post doSync();
    }

    //Log Request
    if(cmd_msg->type == LOG_REQUEST)
    {
      log_head = 0;
      log_tail = 0;
      post readLog();
      call Leds.yellowOn();
    }
    return m;
  }

  event TOS_MsgPtr DataReceiveMsg.receive(TOS_MsgPtr m)
  {
    uint8_t i;
    //keys_t temp;
    KUMsg* recv_msg = (KUMsg*)(m->data);
    
    //dbg(DBG_USR1,"<<<<<<<<<");
    printMsg(recv_msg);

    if(recv_msg->src == SINK_ADDR)
    {
      keys = keys | recv_msg->keys;
      type = FORWARD;

      atomic
      {
        if(1 == event_fwd_flag)
        {
          log_buf[log_tail].lType = EVENT_FWD;
          log_buf[log_tail].lTime = local_clock;
          log_buf[log_tail].lSrc  = TOS_LOCAL_ADDRESS;
          log_buf[log_tail].lDst  = SINK_ADDR;

          log_tail++;
          event_fwd_flag = 0;

          logEncounter();
          post writeLog();
        }
      }

      k_order[0] = 0;

      for(i = 0;i < 32;i++)
      {
        if((i != TOS_LOCAL_ADDRESS * 2) && (i != TOS_LOCAL_ADDRESS * 2 - 1))
        {
          if((keys & (1<<i)) != 0)
          {
            k_order[k_order[0] + 1] = i;
            k_order[0]++;
          }
        }
      }
    }
    else
    {
      keys = keys | recv_msg->keys;
    }

    if((keys & mask) == mask)
    {
      //if((event_uni_flag == 1) && (event_fwd_flag == 1))
      //{ 
        call Leds.greenOn();
        dbg(DBG_USR1,"Done!\n");
      //}

      if(type != FORWARD)
      {
        type = UNI;
        call Leds.yellowOn();

        atomic
        {
          if(event_uni_flag == 1)
          {
            log_buf[log_tail].lType = EVENT_UNI;
            log_buf[log_tail].lTime = local_clock;
            log_buf[log_tail].lSrc  = TOS_LOCAL_ADDRESS;
            log_buf[log_tail].lDst  = recv_msg->src;

            log_tail++;
            event_uni_flag = 0;

            logEncounter();
            post writeLog();
          }
        }
      }
    }
    return m;
  }

  event TOS_MsgPtr LogReceiveMsg.receive(TOS_MsgPtr m)
  {
    LogMsg *temp = (LogMsg*)(m->data);
    memcpy(log_buf + log_tail,temp,sizeof(LogMsg));
    log_tail++;
    //post writeLog();
    return m;
  }

  event result_t LoggerWrite.writeDone(result_t status)
  {
    //dbg(DBG_USR1,"Write Done!%d\n",status);

    call LoggerRead.resetPointer();
    
    call LoggerRead.readNext((uint8_t*)(&temp_log));

    //post writeLog();
    return SUCCESS;
  }

  event result_t LoggerRead.readDone(uint8_t *buffer, result_t success)
  {
    if(SUCCESS)
    {
      dbg(DBG_USR1,"/+++++++++\n");
      printLog((LogMsg*)&temp_log);
      dbg(DBG_USR1,"+++++++++++/\n");
      //call LoggerRead.readNext((uint8_t*)(&temp_log));
    }
    else
    {
      dbg(DBG_USR1,"read fail\n");
    }
    post writeLog();
    memcpy(frags_data.data,buffer,sizeof(LogMsg));
    call LogSendMsg.send(SINK_ADDR,sizeof(LogMsg),&frags_data);

    return SUCCESS;
  }
}
